﻿using System;
using System.Collections.Generic;
using System.Collections;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Net.Sockets;
using System.Threading.Tasks;
using System.Net;
using System.Threading;
using System.IO;
using Microsoft.VisualBasic;
using System.Runtime.InteropServices;
using System.Diagnostics;

//#####################
//需要增加的功能：给unity增加启动检测



namespace learnCsharpwinform
{
    public enum Panels
    {
        XYZ, Joint, Init, Procedure, NotDef
    }

    public partial class Host : Form
    {
        public delegate void InvokeList(string message);
        public delegate void InvokeForm();
        public InvokeForm ThreadToForm;
        // double[] joint = new double[6] { 0, 0, 0, 0, 0, 0 };
        string[] presentLocation = new string[6];
        double[] Initial_Position = { 0, 90, -90, 0, -90, 0 };
        //    double[] Initial_Position = { 0, 0, 0, 0, 0, 0 };
        double[] Expected_Position = { 0, 90, -90, 0, -90, 0 };
        double[] Position_Difference = { 0, 0, 0, 0, 0, 0 };
        SynchronizationContext m_SyncContext = null;
        private static byte[] result = new byte[1024];
        private const int port = 8088;
        private static string IpStr = "127.0.0.1";
        private static Socket serverSocket;
        delegate void SetTextCallback(string text);
        private IkineTestForm TestForm;
        bool StartTest = false;
        int RotateMatrixCount = 0;//rotateX=0;rotateY=1;rotateZ=2;
        double[] CustomFrameValue = new double[6];
        double[] forwardKine = new double[9];
        int NumberOfSolution;

        double toolCenterX, toolCenterY, toolCenterZ, toolCenterRX, toolCenterRY, toolCenterRZ;
        private Panels TopPanel=Panels.Init;
        double[] selfDefinedRPY = new double[6];
        bool worldFrameIsShown = false;
        bool customFrameIsShown = false;
        bool toolFrameIsShown = false;


        double[,] RotateMatrix = new double[4, 4];

        //调试区
        System.Timers.Timer wait1;
        private bool pause = false;

        private int CntForTest = 0;
        bool crash = false;

        //调试区
        private Thread thread1;
        private Programs pro;
        private bot robot;
        private TextBox[] TB;
        private TextBox[] TBXYZ;
        private TextBox[] TBrefXYZ;
        private TextBox[] TBpositionXYZ;
        private TextBox[] TBtoolCenter;
        private TextBox[] TBcustomFrame;
        private Stopwatch stopwatch = new Stopwatch();

        Thread Unity;
        public Host()
        {
            InitializeComponent();
            InitUnity();
            InitSound();
            m_SyncContext = SynchronizationContext.Current;

            robot = new bot();
            // pro = new Programs();
            //timer
            wait1 = new System.Timers.Timer();
            wait1.Interval = 1;
            wait1.Elapsed += new System.Timers.ElapsedEventHandler(timer_Elapsed);

            //直接启动关节刷新

            ThreadToForm = new InvokeForm(forwardKineOperation);
            radioButton2.Checked = true;
            Unity = new Thread(new ThreadStart(RefreshUnity));
            //  ProAction.Interval = 10;
            //ProAction.Enabled = true;
            TB = new TextBox[6] { jointValue1, jointValue2, jointValue3, jointValue4, jointValue5, jointValue6 };
            TBXYZ = new TextBox[6] { tbXYZjointValue1, tbXYZjointValue2, tbXYZjointValue3, tbXYZjointValue4, tbXYZjointValue5, tbXYZjointValue6 };
            TBpositionXYZ = new TextBox[6] { tbPositionX, tbPositionY, tbPositionZ, tbRotateX, tbRotateY, tbRotateZ };
            TBrefXYZ = new TextBox[6] { tbReferX, tbReferY, tbReferZ, tbReferRX, tbReferRY, tbReferRZ };
            TBtoolCenter = new TextBox[6] { tbToolCenterDistanceX, tbToolCenterDistanceY, tbToolCenterDistanceZ, tbToolCenterDistanceRZ, tbToolCenterDistanceRY, tbToolCenterDistanceRX };
            TBcustomFrame = new TextBox[6] { customFrameParaX, customFrameParaY, customFrameParaZ, customFrameParaRZ, customFrameParaRY, customFrameParaRX };
        }



        TreeNode treeNodeClone = new TreeNode();


        int moveName = 1;
        int waitName = 1;
        int stopName = 1;
        int nodeName = 1;


        private void bringtofront(Panels TargetPanel)
        {      
            switch (TargetPanel)
            {
                case Panels.Init: panelInit.BringToFront();TopPanel = Panels.Init; break;
                case Panels.Joint: panelForMovingJoint.BringToFront(); TopPanel = Panels.Joint;  UpdateJoint(false); break;
                case Panels.Procedure: panelForProcedure.BringToFront(); TopPanel = Panels.Procedure; break;
                case Panels.XYZ: panelForMovingXYZ.BringToFront(); TopPanel = Panels.XYZ;  UpdateJoint(false); break;
                default: TopPanel = Panels.NotDef; break;
            }
     
        }

   
        private void procedureToolStripMenuItem_Click(object sender, EventArgs e)
        {
            panelProAll.BringToFront();

            bringtofront(Panels.Procedure);
        }


        private void settingsToolStripMenuItem_Click(object sender, EventArgs e)
        {
            panelSettings.BringToFront();
            TopPanel = Panels.NotDef;
        }

        private void iOToolStripMenuItem_Click(object sender, EventArgs e)
        {
            panelIO.BringToFront();
            TopPanel = Panels.NotDef;
        }

        private void logToolStripMenuItem_Click(object sender, EventArgs e)
        {
            panelLog.BringToFront();
            TopPanel = Panels.NotDef;
        }

        private void initializeToolStripMenuItem_Click(object sender, EventArgs e)
        {
            bringtofront(Panels.Init);
        
        }

        private void robotToolStripMenuItem_Click(object sender, EventArgs e)
        {
            panelIOrobot.BringToFront();
            TopPanel = Panels.NotDef;

        }

        private void mODBUSClientToolStripMenuItem_Click(object sender, EventArgs e)
        {
            panelIOmod.BringToFront();
            TopPanel = Panels.NotDef;
        }

        private void button11_Click(object sender, EventArgs e)
        {


        }

        private void button12_Click(object sender, EventArgs e)
        {

            TreeNode n = new TreeNode("1");
            this.treeView1.Nodes.Add(n);
            TreeNode node1 = new TreeNode("11");
            n.Nodes.Add(node1);
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        /// 

        private void SJTU_Load(object sender, EventArgs e)
        {

            /*  IPAddress ip = IPAddress.Parse(IpStr);
               IPEndPoint ip_end_point = new IPEndPoint(ip, port);
               //创建服务器Socket对象，并设置相关属性  
               serverSocket = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
               //绑定ip和端口  
             serverSocket.Bind(ip_end_point);
               //设置最长的连接请求队列长度  
               serverSocket.Listen(10);
               textBoxSocket.Text = string.Format("启动监听{0}成功", serverSocket.LocalEndPoint.ToString());
               //Console.WriteLine("启动监听{0}成功", serverSocket.LocalEndPoint.ToString());
               //在新线程中监听客户端的连接  
               Thread thread = new Thread(ClientConnectListen);
               thread.Start();
               */
            BotNode.RootNode.Name = "Procedures";

            treeView1.Nodes.Add(BotNode.RootNode);
            treeView1.ExpandAll();

            comboBoxworldOrCustom.SelectedIndex = 0;
            trackBar1.Value = 80;
            CB_RefCoor.SelectedIndex = 0;
            // Unity.Start();
            checkBoxRotate.Checked = true;
            coordinate.Tool = new coordinate(0, 0, 0, 0, 0, 0, Cartesian.RPY);
            coordinate.Tool.Name = "工具坐标系";
            coordinate.List.Add(coordinate.Tool);

            for(int i=0;i<6;i++)
            {
                TBcustomFrame[i].Text = "0";
                TBcustomFrame[i].Enabled= false;
            }
            TB_CustomCoorName.Enabled = false;


        }

        /// <summary>  
        /// 客户端连接请求监听  
        /// </summary>  
        private void ClientConnectListen()
        {
            while (true)
            {
                //为新的客户端连接创建一个Socket对象  
                Socket clientSocket = serverSocket.Accept();
                m_SyncContext.Post(SetTextSafePost, string.Format("客户端{0}成功连接", clientSocket.RemoteEndPoint.ToString()));
                //向连接的客户端发送连接成功的数据  
                ByteBuffer buffer = new ByteBuffer();
                buffer.WriteString("Connected Server");
                clientSocket.Send(WriteMessage(buffer.ToBytes()));
                //每个客户端连接创建一个线程来接受该客户端发送的消息  
                Thread thread = new Thread(RecieveMessage);
                thread.Start(clientSocket);
            }
        }

        /// <summary>  
        /// 数据转换，网络发送需要两部分数据，一是数据长度，二是主体数据  
        /// </summary>  
        /// <param name="message"></param>  
        /// <returns></returns>  
        private static byte[] WriteMessage(byte[] message)
        {
            MemoryStream ms = null;
            using (ms = new MemoryStream())
            {
                ms.Position = 0;
                BinaryWriter writer = new BinaryWriter(ms);
                ushort msglen = (ushort)message.Length;
                writer.Write(msglen);
                writer.Write(message);
                writer.Flush();
                return ms.ToArray();
            }
        }


        private void SetTextSafePost(object text)
        {
            this.textBoxSocket.Text = text.ToString();
        }
        private void SetTextSafePost1(object text)
        {
            this.tbPositionX.Text = text.ToString();
        }

        private void SetTextSafePost2(object text)
        {
            this.tbPositionY.Text = text.ToString();
        }
        private void SetTextSafePost3(object text)
        {
            this.tbPositionZ.Text = text.ToString();
        }
        private void SetTextSafePost4(object text)
        {
            this.tbRotateX.Text = text.ToString();
        }
        private void SetTextSafePost5(object text)
        {
            this.tbRotateY.Text = text.ToString();
        }
        private void SetTextSafePost6(object text)
        {
            this.tbRotateZ.Text = text.ToString();
        }
        /// <summary>  
        /// 接收指定客户端Socket的消息  
        /// </summary>  
        /// <param name="clientSocket"></param>  
        private void RecieveMessage(object clientSocket)
        {
            Socket mClientSocket = (Socket)clientSocket;
            while (true)
            {
                try
                {
                    int receiveNumber = mClientSocket.Receive(result);
                    //  m_SyncContext.Post(SetTextSafePost, string.Format("接收客户端{0}消息， 长度为{1}", mClientSocket.RemoteEndPoint.ToString(), receiveNumber));
                    ByteBuffer buff = new ByteBuffer(result);
                    //数据长度  
                    int len = buff.ReadShort();
                    //数据内容  
                    string data = buff.ReadString();
                    //  m_SyncContext.Post(SetTextSafePost, string.Format("数据内容：{0}", data));
                    Debug.WriteLine("数据内容{0}", data);

                    /*********不通过socket获取内部TCP刷新
                    //分割字符串到六个数据显示框
                    if (data.Length > 30)
                    {
                        string[] sixData = data.Split(new char[] { ' ' });
                        m_SyncContext.Post(SetTextSafePost1, string.Format("{0}", sixData[0]));
                        m_SyncContext.Post(SetTextSafePost2, string.Format("{0}", sixData[1]));
                        m_SyncContext.Post(SetTextSafePost3, string.Format("{0}", sixData[2]));
                        m_SyncContext.Post(SetTextSafePost4, string.Format("{0}", sixData[3]));
                        m_SyncContext.Post(SetTextSafePost5, string.Format("{0}", sixData[4]));
                        m_SyncContext.Post(SetTextSafePost6, string.Format("{0}", sixData[5]));
                    }
                
                */

                }
                catch (Exception ex)
                {
                    m_SyncContext.Post(SetTextSafePost, string.Format(ex.Message));
                    //Console.WriteLine(ex.Message);
                    mClientSocket.Shutdown(SocketShutdown.Both);
                    mClientSocket.Close();
                    break;
                }
            }
        }


        private void button11_Click_1(object sender, EventArgs e)
        {
            TreeNode node1 = new TreeNode("node1");
            BotNode.RootNode.Nodes.Add(node1);
        }

        private void button11_Click_2(object sender, EventArgs e)
        {
            TreeNode one = new TreeNode("hey");
            BotNode.RootNode.Nodes.Add(one);
            BotNode.RootNode.ExpandAll();
        }

        private void movingToolStripMenuItem_Click(object sender, EventArgs e)
        {
        
            panelProAll.BringToFront();
            bringtofront(Panels.Joint);
        }

        private void btnPlay_Click(object sender, EventArgs e)
        {
            btnPlay.Visible = false;
            btnPause.Visible = true;
            BtnStop.Visible = true;
            if (!pause)
            {

                pause = false;
                thread1 = new Thread(new ThreadStart(MBS));
                thread1.Start();

            }
            else
                pause = false;
            //pro.MoveBySequence(robot, wait);
            // robot.movebot(BotNode.RoadPoints[0], wait);


        }


        public void MBS()
        {

            //wait.Interval = 50;

            /*由数据驱动
            for (int i = 0; i < BotNode.NodeAmount - 1; i++)
            {
                robot.movebot(BotNode.RoadPoints[i], wait1);
                Debug.WriteLine("已进入运动循环");
                BotNode.ClearColor();
               BotNode.RootNode.Nodes[i].BackColor = Color.Chocolate; 
                while (wait1.Enabled == true) { Debug.WriteLine("正在运动到第{0}个节点",i+1); }
            }
            */
            //由tag驱动
            foreach (TreeNode rp in BotNode.RootNode.Nodes)
            {
                robot.movebot((double[])rp.Tag, wait1);
                // shared.WaitIsRun = true;
                Debug.WriteLine("已进入运动循环");
                BotNode.ClearColor();
                rp.BackColor = Color.Chocolate;

                while (wait1.Enabled == true)
                //  while (shared.WaitIsRun == true)
                {
                    // Debug.WriteLine("正在运动到第{0}个节点", rp.Index + 1);
                }
            }
            ResetBtn();
            BotNode.ClearColor();
            // Output_Joint(robot.joint);
            CntForTest = 0;
            MessageBox.Show("程序执行完毕", "节卡机器人", MessageBoxButtons.OK, MessageBoxIcon.Information);
        }

        private void ResetBtn()
        {
            btnPlay.BeginInvoke(new Action(() => { btnPlay.Visible = true; }));
            btnPause.BeginInvoke(new Action(() => { btnPause.Visible = false; }));

        }


        private void TimeStart()
        {
            stopwatch.Start();
        }
        private void TimeEnd()
        {
            stopwatch.Stop();

            Debug.WriteLine(stopwatch.ElapsedMilliseconds);
            stopwatch.Reset();
        }



        private void timer1_Operation()
        {

            // shared.test++;
            // Debug.WriteLine(shared.test);

            if (!pause)
            {
                if (shared.CurrentStep > BotNode.NodeCount)
                {
                    shared.CurrentStep = 1;
                    wait1.Enabled = false;
                    //  TimeEnd();

                }
                else
                {
                    lock (this)
                    {
                        shared.CurrentStep++;
                        for (int i = 0; i < 6; i++)
                        {
                            robot.joint[i] = robot.joint[i] + robot.Position_Difference[i];
                        }
                    }
                }

                string ToUnity = -robot.joint[0] + " " + -robot.joint[1] + " " + (-robot.joint[2] - 90) + " " + -robot.joint[3] + " " + -robot.joint[4] + " " + -robot.joint[5];
                axUnityWebPlayer1.SendMessage("formal", "UpdateJoints", ToUnity);
                /*
                      for (int i = 0; i < 6; i++)
                      {
                          axUnityWebPlayer1.SendMessage("help" + string.Format("{0}", i + 1), "SetJ", (robot.joint[i]).ToString("0.000"));

                      }

                      */


                this.Invoke(ThreadToForm);//在主线程中
            }

        }

        private void RefreshUnity()
        {
            while (true)
            {
                for (int i = 0; i < 6; i++)
                {

                    axUnityWebPlayer1.SendMessage("help" + string.Format("{0}", i + 1), "SetJ", (robot.joint[i]).ToString("0.000"));

                }
                Debug.WriteLine("shit");
            }

        }


        private void timer_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {

            // Output_Joint(robot.joint);
            //    wait1.Enabled = false;
            timer1_Operation();

        }


        private void btnPause_Click(object sender, EventArgs e)
        {
            pause = true;
            btnPause.Visible = false;
            btnPlay.Visible = true;


        }

        private void trackBar1_Scroll(object sender, EventArgs e)
        {
            speedPercent.Text = string.Format("{0}%", trackBar1.Value);

        }

        private void 命令ToolStripMenuItem_Click(object sender, EventArgs e)
        {
            panelCommand.BringToFront();
            TopPanel = Panels.Procedure;
        }

        private void 结构ToolStripMenuItem_Click(object sender, EventArgs e)
        {
            panelStructure.BringToFront();
            TopPanel = Panels.Procedure;
        }

        private void 变量ToolStripMenuItem1_Click(object sender, EventArgs e)
        {
            panelVar.BringToFront();
            TopPanel = Panels.Procedure;
        }

        private void button28_Click(object sender, EventArgs e)
        {

            TreeNode move = new TreeNode("move" + moveName);
            move.Name = "move" + moveName;

            BotNode.RootNode.Nodes.Add(move);
            moveName += 1;
            BotNode.RootNode.ExpandAll();
        }

        private void button29_Click(object sender, EventArgs e)
        {

            // Debug.WriteLine(treeView1.Nodes.Count);


            double[] NewNode = new double[6];
            for (int i = 0; i < 6; i++)
            {
                NewNode[i] = robot.joint[i];
            }
            BotNode.RoadPoints.Add(NewNode);

            //输出存储值

            /*  foreach (double[] rp in BotNode.RoadPoints)
              { Output_Joint(rp); }*/
            if (treeView1.SelectedNode == null)
            {
                BotNode node = new BotNode(NewNode, "路点" + BotNode.NodeAmount, null);
            }
            else
            {
                TreeNode currentParent = treeView1.SelectedNode;
                BotNode node = new BotNode(NewNode, "路点" + BotNode.NodeAmount, currentParent);
            }

            //   foreach (TreeNode rp in BotNode.RootNode.Nodes)
            // { Output_Joint((double[])rp.Tag); }

            BotNode.NodeAmount += 1;
            BotNode.RootNode.ExpandAll();
        }

        private void treeView1_NodeMouseClick(object sender, MouseEventArgs e)
        {
            bringtofront(Panels.Procedure);
            /*
            if (radioButton1.Checked)
            {
                if (!treeView1.SelectedNode.Text.Equals("编程区"))

                {
        
             
                    for (int i = 0; i < 6; i++)
                    {
                        axUnityWebPlayer1.SendMessage("help" + string.Format("{0}", i + 1), "SetJ", (((double[])treeView1.SelectedNode.Tag)[i]).ToString("0.000"));
                    }
                 
                }
            }
            */

        }

        private void button30_Click(object sender, EventArgs e)
        {
            TreeNode wait = new TreeNode("wait" + waitName);
            wait.Name = "wait" + waitName;
            if (treeView1.SelectedNode == null)
            {
                BotNode.RootNode.Nodes.Add(wait);

            }
            else
            {
                TreeNode currentParent = treeView1.SelectedNode;
                currentParent.Nodes.Add(wait);

            }
            waitName += 1;
            BotNode.RootNode.ExpandAll();
        }

        private void button31_Click(object sender, EventArgs e)
        {
            TreeNode stop = new TreeNode("stop" + stopName);
            stop.Name = "stop" + stopName;
            if (treeView1.SelectedNode == null)
            {
                BotNode.RootNode.Nodes.Add(stop);

            }
            else
            {
                TreeNode currentParent = treeView1.SelectedNode;
                currentParent.Nodes.Add(stop);

            }
            stopName += 1;
            BotNode.RootNode.ExpandAll();
        }



        private void treeView1_DoubleClick(object sender, TreeNodeMouseClickEventArgs e)
        {
            switch (treeView1.SelectedNode.Name)
            {

                case "move1":
                    checkLoop.Checked = true;
                    ;
                    break;

                default:
                    break;

            }


        }

        private void button36_Click(object sender, EventArgs e)
        {

            treeView1.SelectedNode.Remove();
        }

        private void button35_Click(object sender, EventArgs e)
        {
            int temp = treeView1.SelectedNode.Index;

            if (temp >= 1)
            {
                TreeNode tn = treeView1.SelectedNode;
                TreeNode parent = tn.Parent;
                treeView1.SelectedNode.Remove();
                parent.Nodes.Insert(temp - 1, tn);
            }
        }

        private void button32_Click(object sender, EventArgs e)
        {

            treeView1.SelectedNode.Nodes.Add(treeNodeClone);


        }

        private Point Position = new Point(0, 0);

        private void treeView1_ItemDrag(object sender, ItemDragEventArgs e)
        {
            DoDragDrop(e.Item, DragDropEffects.Move);
        }

        private void treeView1_DragEnter(object sender, DragEventArgs e)
        {
            if (e.Data.GetDataPresent(typeof(TreeNode)))
                e.Effect = DragDropEffects.Move;
            else
                e.Effect = DragDropEffects.None;
        }

        private void treeView1_DragDrop(object sender, DragEventArgs e)
        {
            TreeNode myNode = null;
            if (e.Data.GetDataPresent(typeof(TreeNode)))
            {
                myNode = (TreeNode)(e.Data.GetData(typeof(TreeNode)));
            }
            else
            {
                MessageBox.Show("error");
            }
            Position.X = e.X;
            Position.Y = e.Y;
            Position = treeView1.PointToClient(Position);
            TreeNode DropNode = this.treeView1.GetNodeAt(Position);
            // 1.目标节点不是空。2.目标节点不是被拖拽接点的字节点。3.目标节点不是被拖拽节点本身
            if (DropNode != null && DropNode.Parent != myNode && DropNode != myNode)
            {
                TreeNode DragNode = myNode;
                // 将被拖拽节点从原来位置删除。
                myNode.Remove();
                // 在目标节点下增加被拖拽节点
                DropNode.Nodes.Add(DragNode);
            }
            // 如果目标节点不存在，即拖拽的位置不存在节点，那么就将被拖拽节点放在根节点之下
            if (DropNode == null)
            {
                TreeNode DragNode = myNode;
                myNode.Remove();
                treeView1.Nodes.Add(DragNode);
            }
        }

        private void button38_Click(object sender, EventArgs e)
        {
            int index = 1;
            TreeNode tn = treeView1.Nodes[index];
            treeView1.Nodes.RemoveAt(index);
            treeView1.Nodes.Insert(0, tn);
        }

        private void button33_Click(object sender, EventArgs e)
        {
            int temp = treeView1.SelectedNode.Index;


            TreeNode tn = treeView1.SelectedNode;
            TreeNode parent = tn.Parent;
            treeView1.SelectedNode.Remove();
            parent.Nodes.Insert(temp + 1, tn);

        }

        private void button34_Click(object sender, EventArgs e)
        {
            if (treeView1.SelectedNode != null)
            {
                treeNodeClone = (TreeNode)treeView1.SelectedNode.Clone();
                treeNodeClone.Text = (treeNodeClone.Text + "'");
            }

        }

        private void button37_Click(object sender, EventArgs e)
        {

            if (treeView1.SelectedNode.BackColor == Color.Gray)
            { treeView1.SelectedNode.BackColor = Color.White; }
            else
            {
                treeView1.SelectedNode.BackColor = Color.Gray;
            }

        }

        private void button6_Click(object sender, EventArgs e)
        {
            btnWorking.BringToFront();
            forwardKineOperation();
            string ToUnity = -robot.joint[0] + " " + -robot.joint[1] + " " + (-robot.joint[2] - 90) + " " + -robot.joint[3] + " " + -robot.joint[4] + " " + -robot.joint[5];
            axUnityWebPlayer1.SendMessage("formal", "UpdateJoints", ToUnity);
            radioButton1.PerformClick();
            for (int i = 0; i < 6; i++)
            {
                TB[i].Text = string.Format("{0:f3}", robot.joint[i]);
                TBpositionXYZ[i].Text = string.Format("{0:f3}", robot.pose[i]);

                TBrefXYZ[i].Text = string.Format("{0:f3}", robot.pose[i]);
                TBXYZ[i].Text = string.Format("{0:f3}", robot.joint[i]);
            }
        }

        private void btnWorking_Click(object sender, EventArgs e)
        {
            btnWorking.Enabled = false;
            forwardKineOperation();

        }

        private void BtnJoints_MouseDown(object sender, MouseEventArgs e)
        {
        
            bool add = true;
            string name = ((Button)sender).Name;
            switch(name)
            {
                case "btn1m": robot.SelectedJoint = 0; add = false; break;
                case "btn2m": robot.SelectedJoint = 1; add = false; break;
                case "btn3m": robot.SelectedJoint = 2; add = false; break;
                case "btn4m": robot.SelectedJoint = 3; add = false; break;
                case "btn5m": robot.SelectedJoint = 4; add = false; break;
                case "btn6m": robot.SelectedJoint = 5; add = false; break;
                case "btn1a": robot.SelectedJoint = 0; break;
                case "btn2a": robot.SelectedJoint = 1; break;
                case "btn3a": robot.SelectedJoint = 2; break;
                case "btn4a": robot.SelectedJoint = 3;  break;
                case "btn5a": robot.SelectedJoint = 4;  break;
                case "btn6a": robot.SelectedJoint = 5;  break;

            }
            if(add)
            {
                robot.SelectedTrans = Transfrom.Decrease;
                jointAddTimer.Interval = 20;
                jointAddTimer.Enabled = true;
            }
            else
            {
                robot.SelectedTrans = Transfrom.Increase;
                jointMinusTimer.Interval = 20;
                jointMinusTimer.Enabled=true;
            }
        }
        private void BtnJoints_MouseUp(object sender, MouseEventArgs e)
        {
            jointMinusTimer.Enabled = false;
            jointAddTimer.Enabled = false;
        }

        private void button4_MouseDown(object sender, MouseEventArgs e)
        {
            jointAddTimer.Interval = 20;
            jointAddTimer.Enabled = true;
        }
        private void button4_MouseUp(object sender, MouseEventArgs e)
        {
            jointAddTimer.Enabled = false;
        }

 private void jointMinusTimer_Tick(object sender, EventArgs e)
        {

               if (!crash)
            {
                if (robot.JointIncre())
                { return; }
                if (robot.SelectedJoint == 2)
                { axUnityWebPlayer1.SendMessage("help" + string.Format("{0}", robot.SelectedJoint + 1), "SetJ", (-robot.joint[robot.SelectedJoint] - 90).ToString("0.000")); }
                else
                { axUnityWebPlayer1.SendMessage("help" + string.Format("{0}", robot.SelectedJoint + 1), "SetJ", (-robot.joint[robot.SelectedJoint]).ToString("0.000")); }

                forwardKineOperation();
            }

        }
        private void jointAddTimer_Tick(object sender, EventArgs e)
        {
            if (robot.JointDecre())
            {
                return;
            }
            if (robot.SelectedJoint== 2)
            { axUnityWebPlayer1.SendMessage("help" + string.Format("{0}", robot.SelectedJoint + 1), "SetJ", (-robot.joint[robot.SelectedJoint] - 90).ToString("0.000")); }
            else
            { axUnityWebPlayer1.SendMessage("help" + string.Format("{0}", robot.SelectedJoint + 1), "SetJ", (-robot.joint[robot.SelectedJoint]).ToString("0.000")); }

            forwardKineOperation();
 
        }


        private void forwardKineOperation() //利用robot.joint[i]刷新各个面板的坐标数值,不刷新Unity;
       {
            switch (CB_RefCoor.SelectedIndex)
            {
                case 0:  fkine(robot.joint, robot.ToolSetting, robot.pose); break;
                case 1:fkine(robot.joint, robot.ToolSetting, robot.pose); break;//将末端xyz,rx,ry,rz数值传入fkine
                case 2:
                case 3:
                case 4:
                case 5:
                case 6:
                case 7:
                case 8:
                case 9:
                    //getSelfRPY();//自定义的工具坐标系
                    //self_defining_fkine(selfDefinedRPY, robot.joint, robot.ToolCenter);
                    break;
                default: fkine(robot.joint, new double[] { 0, 0, 0, 0, 0, 0 }, robot.pose);break;

            }
            UpdateJoint(false);
        }
        private void UpdateJoint(bool UpdateAll)
        {
            if (!UpdateAll)
            {
                //传递关节角
                if (TopPanel == Panels.Joint)
                {
                    for (int i = 0; i < 6; i++)
                    {
                        TB[i].Text = string.Format("{0:f3}", robot.joint[i]);
                        TBpositionXYZ[i].Text = string.Format("{0:f3}", robot.pose[i]);
                    }
                }
                else if (TopPanel == Panels.XYZ)
                {
                    for (int i = 0; i < 6; i++)
                    {
                        TBrefXYZ[i].Text = string.Format("{0:f3}", robot.pose[i]);
                        TBXYZ[i].Text = string.Format("{0:f3}", robot.joint[i]);
                    }
                }
            }
            else
            {
                for (int i = 0; i < 6; i++)
                {
                    TB[i].Text = string.Format("{0:f3}", robot.joint[i]);
                    TBpositionXYZ[i].Text = string.Format("{0:f3}", robot.pose[i]);
                    TBrefXYZ[i].Text = string.Format("{0:f3}", robot.pose[i]);
                    TBXYZ[i].Text = string.Format("{0:f3}", robot.joint[i]);
                }
            }
        }

        private void getSelfRPY()
        {

            selfDefinedRPY[0] = Convert.ToDouble(customFrameParaX.Text);
            selfDefinedRPY[1] = Convert.ToDouble(customFrameParaY.Text);
            selfDefinedRPY[2] = Convert.ToDouble(customFrameParaZ.Text);
            selfDefinedRPY[3] = Convert.ToDouble(customFrameParaRZ.Text);
            selfDefinedRPY[4] = Convert.ToDouble(customFrameParaRY.Text);
            selfDefinedRPY[5] = Convert.ToDouble(customFrameParaRX.Text);
        }


        private void btnEdit_Click(object sender, EventArgs e)
        {


            if (tbPositionX.Enabled == false)
            {
                tbPositionX.Enabled = true;
                tbPositionY.Enabled = true;
                tbPositionZ.Enabled = true;
                tbRotateX.Enabled = true;
                tbRotateY.Enabled = true;
                tbRotateZ.Enabled = true;
                presentLocation[0] = tbPositionX.Text;
                presentLocation[1] = tbPositionY.Text;
                presentLocation[2] = tbPositionZ.Text;
                presentLocation[3] = tbRotateX.Text;
                presentLocation[4] = tbRotateY.Text;
                presentLocation[5] = tbRotateZ.Text;

            }
            else
            {
                tbPositionX.Enabled = false;
                tbPositionY.Enabled = false;
                tbPositionZ.Enabled = false;
                tbRotateX.Enabled = false;
                tbRotateY.Enabled = false;
                tbRotateZ.Enabled = false;
                tbPositionX.Text = presentLocation[0];
                tbPositionY.Text = presentLocation[1];
                tbPositionZ.Text = presentLocation[2];
                tbRotateX.Text = presentLocation[3];
                tbRotateY.Text = presentLocation[4];
                tbRotateZ.Text = presentLocation[5];




            }
        }

        private int ComputeDistanceWithJoint(double[] solution)
        {
            int number = 0;
            double distance = 11111110f;
            double temp=0;
            for (int i = 0; i < NumberOfSolution; i++)
            {
                temp = 0;
                for (int j = 0; j < 6; j++)
                {
                    //使用六个关节角之差的平方来判定最近的点
                    temp += (solution[i * 6 + j] - robot.joint [j]) * (solution[i * 6 + j] -robot.joint[j]);
                }
                if (temp < distance)
                {
                    distance = temp;
                    number = i;
                }
             
            }
            return number;
        }




        /// <summary>  
        /// Call method int sum(int, int) defined in Test.dll  
        /// </summary>  
        /// <param name="a">parameter a</param>  
        /// <param name="b">parameter b</param>  
        /// <returns>sum of a and b</returns>  
        [DllImport("testInverseSolutionForRobotDll.dll", EntryPoint = "addAll", CallingConvention = CallingConvention.Cdecl)]
        private static extern int addAll(int a, int b);

        [DllImport("testInverseSolutionForRobotDll.dll", EntryPoint = "subTwo", CallingConvention = CallingConvention.Cdecl)]
        private static extern int subTwo(int a, int b);

        [DllImport("testInverseSolutionForRobotDll.dll", EntryPoint = "inverseSolution", CallingConvention = CallingConvention.Cdecl)]
        private static extern int inverseSolution(double[] z);

        [DllImport("testInverseSolutionForRobotDll.dll", EntryPoint = "inkine", CallingConvention = CallingConvention.Cdecl)]
        private static extern Solution inkine(double[] p, double[] d, double[] solution, double[] joint, double Threshold,double[] speed);

        [DllImport("testInverseSolutionForRobotDll.dll", EntryPoint = "fkine", CallingConvention = CallingConvention.Cdecl)]
        private static extern void fkine(double[] z, double[] d, double[] pose);

        [DllImport("testInverseSolutionForRobotDll.dll", EntryPoint = "self_defining_inkine", CallingConvention = CallingConvention.Cdecl)]
        private static extern int self_defining_inkine(double[] RPY, double[] z, double[] d);

        [DllImport("testInverseSolutionForRobotDll.dll", EntryPoint = "self_defining_fkine", CallingConvention = CallingConvention.Cdecl)]
        private static extern void self_defining_fkine(double[] position, double[] theta, double[] d);


        private void 设置自定义坐标系ToolStripMenuItem_Click(object sender, EventArgs e)
        {
            panelForSettingFrame.BringToFront();
            TopPanel = Panels.Procedure;
        }

        private void 工具系设定ToolStripMenuItem_Click(object sender, EventArgs e)
        {
            panelForSettingToolCenter.BringToFront();
            TopPanel = Panels.Procedure;
        }

        private void button6_Click_1(object sender, EventArgs e)
        {
            if (tbToolCenterDistanceZ.Enabled == false)
            {
              for(int i=0;i<6;i++)
                {
                    TBtoolCenter[i].Enabled = true;
                }
                axUnityWebPlayer1.SendMessage("Main Camera", "DispOriginTool", null);

            }
            else
            {
                try
                {
                    for (int i = 0; i < 6; i++)
                    {
                        robot.ToolSetting[i] = Convert.ToDouble(TBtoolCenter[i].Text);
                    }
                }
                catch (Exception)
                {
                    tbToolCenterDistanceZ.Text = string.Format("{0}", robot.ToolSetting);
                    MessageBox.Show("请正确输入数字");
                    return;
                }

                string ToUnity="";
                for (int i = 0; i < 6; i++)
                {
                    TBtoolCenter[i].Enabled = false;
                    ToUnity += robot.ToolSetting[i];
                    ToUnity += " ";
                }
                toolFrameIsShown = true;
             
                axUnityWebPlayer1.SendMessage("Main Camera", "TransformToolFrame", ToUnity);
                forwardKineOperation();

 axUnityWebPlayer1.SendMessage("Main Camera", "HideOriginTool", null);

            }
        }

        private void button15_Click(object sender, EventArgs e)
        {
            if (coordinate.Num != 0)
            {
                if (customFrameIsShown)
                {
                    axUnityWebPlayer1.SendMessage("Main Camera", "HideCustom", null);
                }
                else
                {
                    axUnityWebPlayer1.SendMessage("Main Camera", "DisplayCustom", null);

                }
                customFrameIsShown = !customFrameIsShown;
            }
            else
            {
                MessageBox.Show("请先建立自定义坐标系！", "错误报告", MessageBoxButtons.OK,
                         MessageBoxIcon.Warning, MessageBoxDefaultButton.Button1);
            }
        }

        private void button11_Click_3(object sender, EventArgs e)
        {
            if (worldFrameIsShown)
            {
                axUnityWebPlayer1.SendMessage("Main Camera", "HideWorld", null);
            }
            else
            {
                axUnityWebPlayer1.SendMessage("Main Camera", "DisplayWorld", null);
            }
            worldFrameIsShown = !worldFrameIsShown;
        }

        private void button16_Click(object sender, EventArgs e)
        {
            if (toolFrameIsShown)
            {
                axUnityWebPlayer1.SendMessage("Main Camera", "HideTool", null);

            }
            else
            {
                axUnityWebPlayer1.SendMessage("Main Camera", "DisplayTool", null);

            }
            toolFrameIsShown = !toolFrameIsShown;
        }

        private void button14_Click(object sender, EventArgs e)
        {
            if (button14.Text == "确认")
            {
                string newName = TB_CustomCoorName.Text;
                foreach (object item in cbPresentFrame.Items)
                {
                    if (item.ToString() == newName)
                    {
                        if (cbPresentFrame.SelectedItem.ToString() != newName)
                        {
                            MessageBox.Show("请勿重复命名!");
                            return;
                        }
                    }
                }
                       double[] newFrame = new double[6];
                try
                {
                    newFrame[0] = Convert.ToDouble(customFrameParaX.Text);
                    newFrame[1] = Convert.ToDouble(customFrameParaY.Text);
                    newFrame[2] = Convert.ToDouble(customFrameParaZ.Text);
                    newFrame[3] = Convert.ToDouble(customFrameParaRX.Text);
                    newFrame[4] = Convert.ToDouble(customFrameParaRY.Text);
                    newFrame[5] = Convert.ToDouble(customFrameParaRZ.Text);
                }

                catch 
                {
                    MessageBox.Show("输入有误，请重新输入");
                    return;
                }

                for (int i = 0; i < 6; i++)
                {
                    TBcustomFrame[i].Enabled = false;
                }
                TB_CustomCoorName.Enabled = false;


                coordinate NewCoor = new coordinate(newFrame, Cartesian.RPY);
                NewCoor.Name = TB_CustomCoorName.Text;
                coordinate.List.Add(NewCoor);//List中List[1]对应cbPresentFrame[0]以及comboworld[1]

                cbPresentFrame.Items.Add(NewCoor.Name);
                comboBoxworldOrCustom.Items.Add(NewCoor.Name);

                cbPresentFrame.SelectedIndex = coordinate.Num - 2;
            
                button14.Text = "新建坐标系";
                button19.Enabled = true;
                button13.Enabled = true;
            }
            else
            {
                for (int i = 0; i < 6; i++)
                {

                    TBcustomFrame[i].Enabled = true;
                    TBcustomFrame[i].Text = "0";
                    TB_CustomCoorName.Text = "自定义" + coordinate.Num;
                }
                TB_CustomCoorName.Enabled = true;
                button14.Text = "确认";
       button19.Enabled = false ;
                button13.Enabled = false;
            }


        }

        private void button13_Click(object sender, EventArgs e)
        {
            if (coordinate.Num > 1)
            {
                //axUnityWebPlayer1.SendMessage("Main Camera", "deleteCustom", null);
                for (int i = 0; i < 6; i++)
                {
                    TBcustomFrame[i].Enabled = false;
                }


                coordinate.Delete(cbPresentFrame.SelectedIndex + 1);
                if(comboBoxworldOrCustom.SelectedItem.ToString()==cbPresentFrame.SelectedItem.ToString())
                {
                    comboBoxworldOrCustom.SelectedIndex = 0;
                }
                comboBoxworldOrCustom.Items.Remove(cbPresentFrame.SelectedItem);
                cbPresentFrame.Items.Remove(cbPresentFrame.SelectedItem);


                if (coordinate.Num > 1)
                { cbPresentFrame.SelectedIndex = coordinate.Num - 2; }
                else if (coordinate.Num == 1)
                {
                    for (int i = 0; i < 6; i++)
                    {
                        TBcustomFrame[i].Text = "0";
                    }

                    TB_CustomCoorName.Text = "";

                    axUnityWebPlayer1.SendMessage("Main Camera", "deleteCustom", null);
                }


            }


            else
            {
                MessageBox.Show("不存在自定义坐标系", "错误报告", MessageBoxButtons.OK,
                           MessageBoxIcon.Warning, MessageBoxDefaultButton.Button1);
            }
        }

        private void button17_Click(object sender, EventArgs e)
        {
            if (jointValue1.Enabled == false)
            {
                jointValue1.Enabled = true;
                jointValue2.Enabled = true;
                jointValue3.Enabled = true;
                jointValue4.Enabled = true;
                jointValue5.Enabled = true;
                jointValue6.Enabled = true;
                presentLocation[0] = jointValue1.Text;
                presentLocation[1] = jointValue2.Text;
                presentLocation[2] = jointValue3.Text;
                presentLocation[3] = jointValue4.Text;
                presentLocation[4] = jointValue5.Text;
                presentLocation[5] = jointValue6.Text;


            }
            else
            {
                jointValue1.Enabled = false;
                jointValue2.Enabled = false;
                jointValue3.Enabled = false;
                jointValue4.Enabled = false;
                jointValue5.Enabled = false;
                jointValue6.Enabled = false;
                jointValue1.Text = presentLocation[0];
                jointValue2.Text = presentLocation[1];
                jointValue3.Text = presentLocation[2];
                jointValue4.Text = presentLocation[3];
                jointValue5.Text = presentLocation[4];
                jointValue6.Text = presentLocation[5];

                forwardKineOperation();
            }
        }

        private void Unity_UpdateAllJoint()
        {
            string ToUnity = -robot.joint[0] + " " + -robot.joint[1] + " " + (-robot.joint[2] - 90) + " " + -robot.joint[3] + " " + -robot.joint[4] + " " + -robot.joint[5];
            axUnityWebPlayer1.SendMessage("formal", "UpdateJoints", ToUnity);

        }

        private void button18_Click(object sender, EventArgs e)
        {
            if (jointValue1.Enabled == true)
            {

                TextBox[] TB = new TextBox[6] { jointValue1, jointValue2, jointValue3, jointValue4, jointValue5, jointValue6 };
                try
                {
                    for (int i = 0; i < 6; i++)
                    {
                        robot.joint[i] = double.Parse(TB[i].Text);
                    }
                }
                catch
                {
                    MessageBox.Show("关节角不对");
                    return;
                }

                Unity_UpdateAllJoint();

                jointValue1.Enabled = false;
                jointValue2.Enabled = false;
                jointValue3.Enabled = false;
                jointValue4.Enabled = false;
                jointValue5.Enabled = false;
                jointValue6.Enabled = false;


                forwardKineOperation();
            }
        }


        private void comboBoxWorldOrCustom_SelectedIndexChanged(object sender, EventArgs e)
        {
            if (toolFrameIsShown) { btnShowToolFrame.PerformClick(); }
            // cbReferenceFrame.SelectedIndex = comboBoxworldOrCustom.SelectedIndex;
            //timer13.Enabled = false;
            if (comboBoxworldOrCustom.SelectedIndex > 0)
            {

                axUnityWebPlayer1.SendMessage("Main Camera", "HideWorld", null);

                axUnityWebPlayer1.SendMessage("Main Camera", "DisplayCustom", null);

            }
            else
            {
                axUnityWebPlayer1.SendMessage("Main Camera", "HideCustom", null);
                axUnityWebPlayer1.SendMessage("Main Camera", "DisplayWorld", null);
            }
            forwardKineOperation();
        }

        private void cbPresentFrame_SelectedIndexChanged(object sender, EventArgs e)
        {
            if (cbPresentFrame.SelectedIndex < 0)
            {
                for (int i = 0; i < 6; i++)
                {
                    TBcustomFrame[i].Text = "0";

                }
                axUnityWebPlayer1.SendMessage("Main Camera", "deleteCustom", null);
            }
            else
            {
                coordinate CustomFrameValue = ((coordinate)(coordinate.List[cbPresentFrame.SelectedIndex + 1]));//rpy[0]是工具系
                for (int i = 0; i < 6; i++)
                {
                    TBcustomFrame[i].Text = string.Format("{0}", CustomFrameValue.val[i]);
                }
                TB_CustomCoorName.Text = CustomFrameValue.Name;

                if (CB_PreviewCustomCoor.Checked == true)
                {

                    axUnityWebPlayer1.SendMessage("Main Camera", "deleteCustom", null);
                    string s = customFrameParaX.Text + " " + customFrameParaY.Text + " " + customFrameParaZ.Text + " " + customFrameParaRX.Text + " " + customFrameParaRY.Text + " " + customFrameParaRZ.Text;
                    axUnityWebPlayer1.SendMessage("Main Camera", "TransformCustomFrame", s);

                }

            }

        }

        private void button19_Click(object sender, EventArgs e)
        {
            if(cbPresentFrame.SelectedIndex<0)
            {
                MessageBox.Show("坐标系为空，不能修改");
                return;
            }
            if (button19.Text == "修改坐标系")
            {
                for (int i = 0; i < 6; i++)
                {
                     TBcustomFrame[i].Enabled = true;
                }
                TB_CustomCoorName.Enabled = true;
                button19.Text = "确认";
                button14.Enabled = false;
                button13.Enabled = false;
            }
            else
            {
                string newName = TB_CustomCoorName.Text;
                foreach (object item in cbPresentFrame.Items)
                {
                    if (item.ToString() == newName)
                    {
                        if (cbPresentFrame.SelectedItem.ToString() != newName)
                        {
                            MessageBox.Show("请勿重复命名!");
                            return;
                        }
                    }
                }
                double[] newFrame = new double[6];
                try
                {
                    newFrame[0] = Convert.ToDouble(customFrameParaX.Text);
                    newFrame[1] = Convert.ToDouble(customFrameParaY.Text);
                    newFrame[2] = Convert.ToDouble(customFrameParaZ.Text);
                    newFrame[3] = Convert.ToDouble(customFrameParaRX.Text);
                    newFrame[4] = Convert.ToDouble(customFrameParaRY.Text);
                    newFrame[5] = Convert.ToDouble(customFrameParaRZ.Text);
                }

                catch 
                {
                    MessageBox.Show("输入有误，请重新输入");
                    return;
                }


                for (int i = 0; i < 6; i++)
                {
                     TBcustomFrame[i].Enabled =false;

                }
                TB_CustomCoorName.Enabled = false;


                button19.Text = "修改坐标系";
                int index = cbPresentFrame.SelectedIndex;
                cbPresentFrame.Items.RemoveAt(index);
                comboBoxworldOrCustom.Items.RemoveAt(index + 1);
                comboBoxworldOrCustom.Items.Insert(index + 1, newName);//List中List[1]对应cbPresentFrame[0]以及comboworld[1]

                coordinate selected = (coordinate)(coordinate.List[index + 1]);
                for (int i = 0; i < 6; i++)
                {
                   selected.val[i] = newFrame[i];

                }
                selected.Name = TB_CustomCoorName.Text;

                cbPresentFrame.Items.Insert(index, newName);
                cbPresentFrame.SelectedIndex = index;


                if (comboBoxworldOrCustom.SelectedIndex == (index + 1))
                {
                    comboBoxworldOrCustom.SelectedIndex = index + 1;
                }

                button14.Enabled = true; 
                button13.Enabled = true ;


            }
        }

        private void button11_Click_4(object sender, EventArgs e)
        {
            btnCreateNode.PerformClick();
        }

        private void jointToolStripMenuItem_Click(object sender, EventArgs e)
        {
          
            bringtofront(Panels.Joint);
            UpdateJoint(false);

        }

        private void xYZToolStripMenuItem_Click(object sender, EventArgs e)
        {
            bringtofront(Panels.XYZ);
            UpdateJoint(false);
        }



        private void button15_Click_1(object sender, EventArgs e)
        {
            btnCreateNode.PerformClick();
        }

        private void button21_Click(object sender, EventArgs e)
        {
            if (tbReferX.Enabled == false)
            {
                tbReferX.Enabled = true;
                tbReferY.Enabled = true;
                tbReferZ.Enabled = true;
                tbReferRX.Enabled = true;
                tbReferRY.Enabled = true;
                tbReferRZ.Enabled = true;

                presentLocation[0] = tbReferX.Text;
                presentLocation[1] = tbReferY.Text;
                presentLocation[2] = tbReferZ.Text;
                presentLocation[3] = tbReferRX.Text;
                presentLocation[4] = tbReferRY.Text;
                presentLocation[5] = tbReferRZ.Text;

            }
            else
            {
                tbReferX.Enabled = false;
                tbReferY.Enabled = false;
                tbReferZ.Enabled = false;
                tbReferRX.Enabled = false;
                tbReferRY.Enabled = false;
                tbReferRZ.Enabled = false;

                tbReferX.Text = presentLocation[0];
                tbReferY.Text = presentLocation[1];
                tbReferZ.Text = presentLocation[2];
                tbReferRX.Text = presentLocation[3];
                tbReferRY.Text = presentLocation[4];
                tbReferRZ.Text = presentLocation[5];


            }





        }

        private void button20_Click_1(object sender, EventArgs e)
        {
            if (tbReferX.Enabled == true)
            {

                double[] getSolution = new double[48];
                getSolution[0] = Convert.ToDouble(tbReferX.Text);
                getSolution[1] = Convert.ToDouble(tbReferY.Text);
                getSolution[2] = Convert.ToDouble(tbReferZ.Text);
                getSolution[3] = Convert.ToDouble(tbReferRX.Text);
                getSolution[4] = Convert.ToDouble(tbReferRY.Text);
                getSolution[5] = Convert.ToDouble(tbReferRZ.Text);

                if (comboBoxworldOrCustom.SelectedIndex > 0)
                {
                    getSelfRPY();
                    NumberOfSolution = self_defining_inkine(selfDefinedRPY, getSolution, robot.ToolSetting);
                }
                else
                {
                  //  NumberOfSolution = inkine(robot.pose, robot.ToolSetting, robot.solution);
                }
                //double[] changeScope = new double[48];
                //changeScope = Array.ConvertAll(getSolution, d => d);


                //如果无解，说明超出工作空间
                if (NumberOfSolution == 0)
                {
                    DialogResult dr;
                    dr = MessageBox.Show("超出工作空间！", "错误报告", MessageBoxButtons.OK,
                             MessageBoxIcon.Warning, MessageBoxDefaultButton.Button1);
                    btnEditXYZ.PerformClick();

                }
                else
                {



                    double[,] solution = new double[8, 6];
                    for (int i = 0; i < 8; i++)
                    {
                        for (int j = 0; j < 6; j++)
                        {
                            solution[i, j] = getSolution[6 * i + j];
                        }
                    }


                    int minIndex = ComputeDistanceWithJoint(robot.solution);

                    double[] jointValue = new double[6];
                    for (int i = 0; i < 6; i++)
                    {
                        //joint[i] = solution[minIndex, i];

                        jointValue[i] = solution[minIndex, i];
                        if (i == 2) { robot.joint[i] = -jointValue[i] - 90; }
                        else
                        {
                            robot.joint[i] = -jointValue[i];
                        }
                        axUnityWebPlayer1.SendMessage("help" + string.Format("{0}", i + 1), "SetJ", (robot.joint[i]).ToString("0.000"));
                    }


                }




                tbReferX.Enabled = false;
                tbReferY.Enabled = false;
                tbReferZ.Enabled = false;
                tbReferRX.Enabled = false;
                tbReferRY.Enabled = false;
                tbReferRZ.Enabled = false;


         
            }
        }

        private double cos(double angle)
        {
            return Math.Cos(angle / 180 * Math.PI);
        }

        private double sin(double angle)
        {
            return Math.Sin(angle / 180 * Math.PI);
        }

        public double[,] matrixMultipy(double[,] a, double[,] b)
        {
            double[,] multipy = new double[4, 4];
            for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                {
                    multipy[i, j] = a[i, 0] * b[0, j] + a[i, 1] * b[1, j] + a[i, 2] * b[2, j] + a[i, 3] * b[3, j];

                }
            return multipy;
        }

        public double[] MatrixToRPYold(double[,] T)
        {
            double[] newRPY = new double[6];
            newRPY[0] = T[0, 3];
            newRPY[1] = T[1, 3];
            newRPY[2] = T[2, 3];
            if (Math.Sqrt(T[0, 0] * T[0, 0] + T[1, 0] * T[1, 0]) != 0)
            {
                newRPY[4] = Math.Atan2(-T[2, 0], Math.Sqrt(T[0, 0] * T[0, 0] + T[1, 0] * T[1, 0]));
                newRPY[3] = Math.Atan2(T[2, 1] / Math.Cos(newRPY[4]), T[2, 2] / Math.Cos(newRPY[4]));
                newRPY[5] = Math.Atan2(T[1, 0] / Math.Cos(newRPY[4]), T[0, 0] / Math.Cos(newRPY[4]));
            }
            else
            {
                newRPY[3] = Math.Atan2(T[1, 0], T[0, 0]);
                newRPY[4] = Math.PI / 2;
                newRPY[5] = 0;
            }
            newRPY[3] = newRPY[3] / Math.PI * 180;
            newRPY[4] = newRPY[4] / Math.PI * 180;
            newRPY[5] = newRPY[5] / Math.PI * 180;


            return newRPY;
        }
        public double[] MatrixToRPY(double[,] T)
        {
            double[] newRPY = new double[48];
            newRPY[0] = T[0, 3];
            newRPY[1] = T[1, 3];
            newRPY[2] = T[2, 3];

            newRPY[3] = Math.Atan2(T[1, 0], T[0, 0]);
            newRPY[4] = Math.Atan2(-T[2, 0], Math.Cos(newRPY[3]) * T[0, 0] + Math.Sin(newRPY[3]) * T[1, 0]);
            newRPY[5] = Math.Atan2(Math.Sin(newRPY[3]) * T[0, 2] - Math.Cos(newRPY[3]) * T[1, 2], -Math.Sin(newRPY[3]) * T[0, 1] + Math.Cos(newRPY[3]) * T[1, 1]);

            newRPY[3] = newRPY[3] / Math.PI * 180;
            newRPY[4] = newRPY[4] / Math.PI * 180;
            newRPY[5] = newRPY[5] / Math.PI * 180;

            return newRPY;
        }



        //private void inverseOperationold()
        //{
        //    if (CB_RefCoor.SelectedIndex == 0)
        //    {
        //        NumberOfSolution = inkine(robot.pose,  robot.ToolSetting, robot.solution);
        //    }
        //    else if (CB_RefCoor.SelectedIndex == 1)
        //    {
        //        switch (robot.SelectedCar)
        //        {
        //            case Cartesian.x:
        //            case Cartesian.y:
        //            case Cartesian.z:
        //                NumberOfSolution = ToolIkine(robot.pose, robot.ToolSetting, robot.SelectedCar, robot.StepDistance, robot.solution);//获取逆解个数
        //                break;
        //            case Cartesian.rotx:
        //            case Cartesian.roty:
        //            case Cartesian.rotz:
        //                NumberOfSolution = ToolIkine(robot.pose, robot.ToolSetting, robot.SelectedCar, robot.StepAngle, robot.solution);//获取逆解个数

        //                break;

        //        }
        //    }


        //    else
        //    {
        //        getSelfRPY();
        //        NumberOfSolution = self_defining_inkine(selfDefinedRPY, robot.pose, robot.ToolSetting);
        //    }
        //    //double[] changeScope = new double[48];
        //    //changeScope = Array.ConvertAll(getSolution, d => d);


        //    //如果无解，说明超出工作空间
        //    if (StartTest)
        //    {
        //        TestForm.ShowIkineNum(NumberOfSolution);
        //    }

        //    if (NumberOfSolution == 0)
        //    {
        //        //DialogResult dr;
        //        //dr = MessageBox.Show("超出工作空间！", "错误报告", MessageBoxButtons.OK,
        //        //         MessageBoxIcon.Warning, MessageBoxDefaultButton.Button1);
        //        //btnEditXYZ.PerformClick();
        //        xyztimer.Enabled = false;
        //        Debug.WriteLine("无解" + DateTime.Now.ToString());
        //        forwardKineOperation();
        //    }
        //    else
        //    {
        //        if (StartTest)
        //        {
        //            double[,] solution = new double[8, 6];
        //            for (int i = 0; i < 8; i++)
        //            {
        //                for (int j = 0; j < 6; j++)
        //                {
        //                    solution[i, j] = robot.solution[6 * i + j];
        //                }
        //            }

        //            TestForm.ShowDGV(solution);
        //        }
        //        int minIndex = ComputeDistanceWithJoint(robot.solution);//solution是tb的XYZ和RX,RY,RZ值
        //        double[] EachJoint = new double[6];
        //        for (int i = 0; i < 6; i++)
        //        {
        //            //joint[i] = solution[minIndex, i];

        //            EachJoint[i] = robot.solution[minIndex * 6 + i];

        //        }
        //        bool IsReasonable = true;

        //        if (CB_Singularity.Checked == true)
        //        {
        //            if (Math.Abs(EachJoint[3] - robot.joint[3]) > 5)
        //            {
        //                if (Math.Abs(EachJoint[4]) < 10)
        //                {
        //                    Debug.WriteLine("奇异" + Math.Abs(EachJoint[3] - robot.joint[3]) + DateTime.Now.ToString());

        //                    EachJoint[3] = robot.joint[3];
        //                    EachJoint[5] = EachJoint[5] + robot.joint[3];

        //                }
        //            }

        //            for (int cnt = 0; cnt < 6; cnt++)
        //            {
        //                if (Math.Abs(EachJoint[cnt] - robot.joint[cnt]) > 10)
        //                {
        //                    IsReasonable = false;

        //                    Debug.WriteLine("关节" + cnt + "跳变" + Math.Abs(EachJoint[3] - robot.joint[3]) + DateTime.Now.ToString());
        //                    forwardKineOperation();
        //                    return;
        //                }
        //            }
        //        }


        //        if (IsReasonable)
        //        {
        //            for (int i = 0; i < 6; i++)
        //            {
        //                robot.joint[i] = EachJoint[i];
        //            }
        //            string ToUnity = -robot.joint[0] + " " + -robot.joint[1] + " " + (-robot.joint[2] - 90) + " " + -robot.joint[3] + " " + -robot.joint[4] + " " + -robot.joint[5];
        //            axUnityWebPlayer1.SendMessage("formal", "UpdateJoints", ToUnity);


        //        }


        //        UpdateJoint(false);


        //    }

        //}



        private void inverseOperation()//已经去除了TIMER
        {
            if (StartTest)
            {
                TestForm.ShowIkineNum((int)robot.Test[1]);
                  double[,] solution = new double[8, 6];
                    for (int i = 0; i < 8; i++)
                    {
                        for (int j = 0; j < 6; j++)
                        {
                            solution[i, j] = robot.solution[6 * i + j];
                        }
                    }
                    TestForm.ShowDGV(solution);
            }
            if (robot.IkineResult != Solution.OK)
            {
                string warning = "";
                switch (robot.IkineResult)
                {
                    case Solution.HighSpeed: warning = "超速警告"; break;
                    case Solution.OutOfRange: warning = "即将超出关节界限"; break;
                    case Solution.Singular: warning = "请避开奇异点！"; break;
                    case Solution.Unkown: warning = "到达极限位置"; break;
                }
                xyztimer.Enabled = false;
                MessageBox.Show(warning);
                return;
            }
            else
            {
                for (int i = 0; i < 6; i++)
                {
                    robot.pose[i] = robot.TempPose[i];
                }

                string ToUnity = -robot.joint[0] + " " + -robot.joint[1] + " " + (-robot.joint[2] - 90) + " " + -robot.joint[3] + " " + -robot.joint[4] + " " + -robot.joint[5];
                axUnityWebPlayer1.SendMessage("formal", "UpdateJoints", ToUnity);
                UpdateJoint(false);
            }
        }
        private void xyztimer_Tick(object sender, EventArgs e)
        {
            robot.teach();
            inverseOperation();

        }
   private void PanforXYZ_MouseDown(object sender, MouseEventArgs e)
        {
            string name = ((Button)sender).Name;
            if (checkBoxRotate.Checked == true)
            {
                switch (name)
                {
                    case "btnAX": robot.SelectedCar = Cartesian.x; robot.SelectedTrans = Transfrom.Increase; break;
                    case "btnAY": robot.SelectedCar = Cartesian.y; robot.SelectedTrans = Transfrom.Increase; break;
                    case "btnAZ": robot.SelectedCar = Cartesian.z; robot.SelectedTrans = Transfrom.Increase; break;
                    case "btnARX": robot.SelectedCar = Cartesian.rotx; robot.SelectedTrans = Transfrom.Increase; break;
                    case "btnARY": robot.SelectedCar = Cartesian.roty; robot.SelectedTrans = Transfrom.Increase; break;
                    case "btnARZ": robot.SelectedCar = Cartesian.rotz; robot.SelectedTrans = Transfrom.Increase; break;
                    case "btnMX": robot.SelectedCar = Cartesian.x; robot.SelectedTrans = Transfrom.Decrease; break;
                    case "btnMY": robot.SelectedCar = Cartesian.y; robot.SelectedTrans = Transfrom.Decrease; break;
                    case "btnMZ": robot.SelectedCar = Cartesian.z; robot.SelectedTrans = Transfrom.Decrease; break;
                    case "btnMRX": robot.SelectedCar = Cartesian.rotx; robot.SelectedTrans = Transfrom.Decrease; break;
                    case "btnMRY": robot.SelectedCar = Cartesian.roty; robot.SelectedTrans = Transfrom.Decrease; break;
                    case "btnMRZ": robot.SelectedCar = Cartesian.rotz; robot.SelectedTrans = Transfrom.Decrease; break;
                }
            }
            else
            {
                switch (name)
                {
                    case "btnAX": robot.SelectedCar = Cartesian.x; robot.SelectedTrans = Transfrom.Increase; break;
                    case "btnAY": robot.SelectedCar = Cartesian.y; robot.SelectedTrans = Transfrom.Increase; break;
                    case "btnAZ": robot.SelectedCar = Cartesian.z; robot.SelectedTrans = Transfrom.Increase; break;
                    case "btnARX": robot.SelectedCar = Cartesian.rot; robot.SelectedTrans = Transfrom.Increase; break;
                    case "btnARY": robot.SelectedCar = Cartesian.pitch; robot.SelectedTrans = Transfrom.Increase; break;
                    case "btnARZ": robot.SelectedCar = Cartesian.yaw; robot.SelectedTrans = Transfrom.Increase; break;
                    case "btnMX": robot.SelectedCar = Cartesian.x; robot.SelectedTrans = Transfrom.Decrease; break;
                    case "btnMY": robot.SelectedCar = Cartesian.y; robot.SelectedTrans = Transfrom.Decrease; break;
                    case "btnMZ": robot.SelectedCar = Cartesian.z; robot.SelectedTrans = Transfrom.Decrease; break;
                    case "btnMRX": robot.SelectedCar = Cartesian.rot; robot.SelectedTrans = Transfrom.Decrease; break;
                    case "btnMRY": robot.SelectedCar = Cartesian.pitch; robot.SelectedTrans = Transfrom.Decrease; break;
                    case "btnMRZ": robot.SelectedCar = Cartesian.yaw; robot.SelectedTrans = Transfrom.Decrease; break;
                }

            }
            xyztimer.Interval = 20;
            xyztimer.Enabled = true;
        }

   private void PanforXYZ_MouseUp(object sender, MouseEventArgs e)
        {
          
            xyztimer.Enabled = false;
        }
    

  



        public void changeRotateData(Boolean add)
        {
            double[] getSolution = new double[6];
            getSolution[0] = Convert.ToDouble(tbReferX.Text);
            getSolution[1] = Convert.ToDouble(tbReferY.Text);
            getSolution[2] = Convert.ToDouble(tbReferZ.Text);
            getSolution[3] = Convert.ToDouble(tbReferRX.Text);
            getSolution[4] = Convert.ToDouble(tbReferRY.Text);
            getSolution[5] = Convert.ToDouble(tbReferRZ.Text);
            double theta, fan, fi;
            theta = getSolution[4];
            fi = getSolution[5];
            fan = getSolution[3];
            double[,] position = new double[4, 4];
            position[0, 3] = getSolution[0];
            position[1, 3] = getSolution[1];
            position[2, 3] = getSolution[2];
            position[3, 3] = 1;
            position[3, 0] = 0;
            position[3, 1] = 0;
            position[3, 2] = 0;
            position[0, 0] = cos(fan) * cos(theta);
            position[1, 0] = sin(fan) * cos(theta);
            position[2, 0] = -sin(theta);
            position[0, 1] = cos(fan) * sin(theta) * sin(fi) - sin(fan) * cos(fi);
            position[1, 1] = sin(fan) * sin(theta) * sin(fi) + cos(fan) * cos(fi);
            position[2, 1] = cos(theta) * sin(fi);
            position[0, 2] = cos(fan) * sin(theta) * cos(fi) + sin(fan) * sin(fi);
            position[1, 2] = sin(fan) * sin(theta) * cos(fi) - cos(fan) * sin(fi);
            position[2, 2] = cos(theta) * cos(fi);


            double delta = Convert.ToDouble(tbXYZjointSpeed.Text) * trackBar1.Value / 5000;
            if (!add) { delta = -delta; }
            switch (RotateMatrixCount)
            {
                case 0:
                    RotateMatrix[0, 0] = 1;
                    RotateMatrix[0, 1] = 0;
                    RotateMatrix[0, 2] = 0;
                    RotateMatrix[0, 3] = 0;
                    RotateMatrix[1, 0] = 0;
                    RotateMatrix[1, 1] = cos(delta);
                    RotateMatrix[1, 2] = -sin(delta);
                    RotateMatrix[1, 3] = 0;
                    RotateMatrix[2, 0] = 0;
                    RotateMatrix[2, 1] = sin(delta);
                    RotateMatrix[2, 2] = cos(delta);
                    RotateMatrix[2, 3] = 0;
                    RotateMatrix[3, 0] = 0;
                    RotateMatrix[3, 1] = 0;
                    RotateMatrix[3, 2] = 0;
                    RotateMatrix[3, 3] = 1;

                    break;
                case 1:
                    RotateMatrix[0, 0] = cos(delta);
                    RotateMatrix[0, 1] = 0;
                    RotateMatrix[0, 2] = sin(delta);
                    RotateMatrix[0, 3] = 0;
                    RotateMatrix[1, 0] = 0;
                    RotateMatrix[1, 1] = 1;
                    RotateMatrix[1, 2] = 0;
                    RotateMatrix[1, 3] = 0;
                    RotateMatrix[2, 0] = -sin(delta);
                    RotateMatrix[2, 1] = 0;
                    RotateMatrix[2, 2] = cos(delta);
                    RotateMatrix[2, 3] = 0;
                    RotateMatrix[3, 0] = 0;
                    RotateMatrix[3, 1] = 0;
                    RotateMatrix[3, 2] = 0;
                    RotateMatrix[3, 3] = 1;
                    break;
                case 2:
                    RotateMatrix[0, 0] = cos(delta);
                    RotateMatrix[0, 1] = -sin(delta);
                    RotateMatrix[0, 2] = 0;
                    RotateMatrix[0, 3] = 0;
                    RotateMatrix[1, 0] = sin(delta);
                    RotateMatrix[1, 1] = cos(delta);
                    RotateMatrix[1, 2] = 0;
                    RotateMatrix[1, 3] = 0;
                    RotateMatrix[2, 0] = 0;
                    RotateMatrix[2, 1] = 0;
                    RotateMatrix[2, 2] = 1;
                    RotateMatrix[2, 3] = 0;
                    RotateMatrix[3, 0] = 0;
                    RotateMatrix[3, 1] = 0;
                    RotateMatrix[3, 2] = 0;
                    RotateMatrix[3, 3] = 1;
                    break;
                default: break;
            }

            position = matrixMultipy(RotateMatrix, position);
            //for(int i=0;i<4;i++)
            //    for(int j=0;j<4;j++)
            //textBox3.Text += string.Format("{0},", position[i, j]);
            //此处添加矩阵转RPY程序，position中数据传给getSolution
            getSolution = MatrixToRPY(position);
            tbReferX.Text = string.Format("{0}", getSolution[0]);
            tbReferY.Text = string.Format("{0}", getSolution[1]);
            tbReferZ.Text = string.Format("{0}", getSolution[2]);
            tbReferRX.Text = string.Format("{0}", getSolution[3]);
            tbReferRY.Text = string.Format("{0}", getSolution[4]);
            tbReferRZ.Text = string.Format("{0}", getSolution[5]);

        }

        private void btn1m_Click(object sender, EventArgs e)
        {

        }

        private void tbPositionY_TextChanged(object sender, EventArgs e)
        {

        }

        private void tbPositionZ_TextChanged(object sender, EventArgs e)
        {

        }

        private void btn2m_Click(object sender, EventArgs e)
        {

        }

        private void btn3m_Click(object sender, EventArgs e)
        {

        }

        private void btnAX_Click(object sender, EventArgs e)
        {

        }

        private void button9_Click(object sender, EventArgs e)
        {
            DispTcpMessage("正在初始化TCP连接");
            Application.DoEvents();
            Thread ThreadForInial = new Thread(InitTcpCon); //如果直接调用IniTcpCon,如果长时间没连接上，会耽误主线程时间。
            ThreadForInial.Start();

        }

        private void InitTcpCon()
        {
            TcpConn tcpconn = new TcpConn();
            tcpconn.Dele_InvokeForm += new TcpConn.InvokeForm(DispTcpMessage);
            tcpconn.InitConn();

        }


        private void DispTcpMessage(string message)
        {

            if (List_TcpConn.InvokeRequired)
            {
                Application.DoEvents();
                InvokeList il;
                il = new InvokeList(DispTcpMessage);
                Invoke(il, message);


            }
            else

            {
                List_TcpConn.Items.Add(message);
                Application.DoEvents();
            }
        }


        private void button10_Click(object sender, EventArgs e)
        {

        }

        private void button8_Click(object sender, EventArgs e)
        {

        }

        private void button7_Click(object sender, EventArgs e)
        {

        }

        private void btnAZ_Click(object sender, EventArgs e)
        {

        }

        private void label24_Click(object sender, EventArgs e)
        {

        }
        //调试区
        private void Btn_Reset_Click(object sender, EventArgs e)
        {

            wait1.Enabled = false;
            robot.movebot(robot.Initial_Position, wait1);
            //TimeStart();

            //Thread td_Moving = new Thread(Reset_Position);
            //td_Moving.Start();

            //movebot(Initial_Position);

            //  Output_Joint(robot.Position_Difference);
        }


        private void movebot(double[] Expected_Position)
        {

            for (int cnt = 0; cnt < 6; cnt++)
            { Position_Difference[cnt] = (Expected_Position[cnt] - robot.joint[cnt]) / 100; }
            //test  Output_Joint(Position_Difference);

            if (Compare_Position(robot.joint, Expected_Position))
            { }
            else
                wait.Enabled = true;

        }




        /// <summary>
        /// 
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void wait_Tick(object sender, EventArgs e)
        {
            stopwatch.Stop();
            shared.TimeTest = stopwatch.ElapsedMilliseconds;
            Debug.WriteLine(shared.TimeTest);
            stopwatch.Reset();
            // wait.Enabled = false;
            // Stopwatch TimeInterval = new Stopwatch();
            //TimeInterval.Start();
            //shared.test++;
            //  Debug.WriteLine(shared.test);
            //if (robot.Compare_Position(robot.joint,robot.ExPos))
            if (shared.CurrentStep > 100)
            {
                shared.CurrentStep = 1;
                wait.Enabled = false;

            }

            else
            {
                shared.CurrentStep++;

                for (int i = 0; i < 6; i++)
                {
                    robot.joint[i] = robot.joint[i] + robot.Position_Difference[i];
                    forwardKineOperation();

                    // Output_Joint(Position_Difference);
                    // Output_Joint(joint);
                    /*   if (i == 2) { joint[i] = (-joint[i] - 90); }
                       else
                       {
                           joint[i] = -joint[i];
                       }
                       */
                    axUnityWebPlayer1.SendMessage("help" + string.Format("{0}", i + 1), "SetJ", (robot.joint[i]).ToString("0.000"));
                }

                //  wait.Enabled = true;
            }

            stopwatch.Start();
            //            TimeInterval.Stop();
            //          Output_Joint(robot.joint, TimeInterval.Elapsed.TotalMilliseconds.ToString());
        }

        private void Output_Joint(double[] joint)
        {
            string Output_Jvalue = "jointvalue";
            CntForTest += 1;
            foreach (double Joint_value in joint)
            {
                Output_Jvalue = Output_Jvalue + "/" + Joint_value.ToString();
            }
            Debug.WriteLine("第{0}个点" + Output_Jvalue, CntForTest);
        }
        //OutputJoint重载
        private void Output_Joint(double[] joint, string message)
        {
            string Output_Jvalue = "jointvalue";
            CntForTest += 1;
            foreach (double Joint_value in joint)
            {
                Output_Jvalue = Output_Jvalue + "/" + Joint_value.ToString();
            }
            Output_Jvalue += "/共" + message + "mm";
            Debug.WriteLine("第{0}个点" + Output_Jvalue, CntForTest);
        }



        private bool Compare_Position(double[] Current_Position, double[] Expected_Position)
        {
            bool JointIsReset = true;
            for (int cnt = 0; cnt < 6; cnt++)
            {
                double error;
                error = Math.Abs(Current_Position[cnt] - Expected_Position[cnt]);
                if (error > 0.01)
                {
                    //  Debug.WriteLine(error.ToString());
                    //Debug.WriteLine("未达到初始位置");
                    JointIsReset = false;
                }

            }
            return JointIsReset;
        }

        //调试区


        private void button26_Click(object sender, EventArgs e)
        {
            // stop = true;

            thread1.Abort();
            wait1.Enabled = false;

            BotNode.ClearColor();
            pause = false;
            shared.CurrentStep = 1;
            btnPlay.Visible = true;
            btnPause.Visible = false;
            CntForTest = 0;
        }



        private void 示教ToolStripMenuItem_Click(object sender, EventArgs e)
        {

        }

        private void ProAction_Tick(object sender, EventArgs e)
        {
            RefreshUnity();
        }

        private void radioButton2_CheckedChanged(object sender, EventArgs e)
        {

        }


        private void List_TcpConn_SelectedIndexChanged(object sender, EventArgs e)
        {

        }

        private void radioButton1_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton1.Checked == true)
            {
                axUnityWebPlayer1.SendMessage("Main Camera", "DisplayTool", null);
                axUnityWebPlayer1.SendMessage("Main Camera", "DisplayWorld", null);
            }
        }

        private void customFrame_KeyPress(object sender, KeyPressEventArgs e)
        {

            if (e.KeyChar == (char)Keys.Enter)
            {
                


            }
        }

        private void customFrameParaZ_TextChanged(object sender, EventArgs e)
        {

        }

        private void CB_PreviewCustomCoor_CheckedChanged(object sender, EventArgs e)
        {

            if (cbPresentFrame.SelectedIndex >= 0)
            {
                if (CB_PreviewCustomCoor.Checked == true)
                {
                    axUnityWebPlayer1.SendMessage("Main Camera", "deleteCustom", null);

                    string s = customFrameParaX.Text + " " + customFrameParaY.Text + " " + customFrameParaZ.Text + " " + customFrameParaRX.Text + " " + customFrameParaRY.Text + " " + customFrameParaRZ.Text;
                    axUnityWebPlayer1.SendMessage("Main Camera", "TransformCustomFrame", s);
                }
                else
                {
                    if (cbPresentFrame.SelectedItem.ToString() != comboBoxworldOrCustom.SelectedItem.ToString())
                    { axUnityWebPlayer1.SendMessage("Main Camera", "deleteCustom", null); }
                }

            }

        }

        private void tbToolCenter_TextChanged(object sender, EventArgs e)
        {
            if (((TextBox)sender).Enabled == true)
            {

                try
                {
                    for (int i = 0; i < 6; i++)
                    {
                        robot.ToolSetting[i] = Convert.ToDouble(TBtoolCenter[i].Text);
                    }
                  
                }
                catch (Exception)
                {
                 
                    return;
                }

                string ToUnity="";
                for (int i = 0; i < 6; i++)
                {
                    ToUnity += robot.ToolSetting[i];
                    ToUnity += " ";
                }
                toolFrameIsShown = true;


                axUnityWebPlayer1.SendMessage("Main Camera", "TransformToolFrame", ToUnity);
            }

        }

        private void customFrame_TextChanged(object sender, EventArgs e)
        {
            if (((TextBox)sender).Enabled == true)
            {
                if (CB_PreviewCustomCoor.Checked == true)
                {
                    double[] custom=new double[6];

                    try
                    {
                        for (int i = 0; i < 6; i++)
                        {
                            custom[i] = Convert.ToDouble(TBcustomFrame[i].Text);
                        }

                    }
                    catch (Exception)
                    {

                        return;
                    }

                    string ToUnity = "";
                    for (int i = 0; i < 6; i++)
                    {
                        ToUnity += custom[i];
                        ToUnity += " ";
                    }
             
              axUnityWebPlayer1.SendMessage("Main Camera", "deleteCustom", null);
                    axUnityWebPlayer1.SendMessage("Main Camera", "TransformCustomFrame", ToUnity);
                }

            }


        }

        private void CB_RefCoor_SelectedIndexChanged(object sender, EventArgs e)
        {
            //checkBoxRotate.Enabled = true;
            if (CB_RefCoor.SelectedIndex==0)
            {
                robot.RefCoor = Transfrom.World;

            }
            else if(CB_RefCoor.SelectedIndex==1)
            {
                robot.RefCoor = Transfrom.Tool;
                //checkBoxRotate.Checked = true;
                //checkBoxRotate.Enabled = false;
            }
        }

        private void tbMaxXYZspeed_TextChanged(object sender, EventArgs e)
        {

        }

        private void Btn_IkineTest_Click(object sender, EventArgs e)
        {
            StartTest = true;
            TestForm = new IkineTestForm();
            TestForm.Show();
        }

        private void trackBar1_ValueChanged(object sender, EventArgs e)
        {
            speedPercent.Text = string.Format("{0}%", trackBar1.Value);
            robot.StepAngle = double.Parse(tbMaxJointSpeed.Text) * trackBar1.Value / 5000;
            robot.StepDistance = Convert.ToDouble(tbMaxXYZspeed.Text) * trackBar1.Value / 5000;
        }

        private void CB_Singularity_CheckedChanged(object sender, EventArgs e)
        {

        }

        private void checkBoxRotate_CheckedChanged(object sender, EventArgs e)
        {
            if (checkBoxRotate.Checked)
            {
                btnARZ.Text = "Z+";
                btnMRZ.Text = "Z-";
                btnARY.Text = "Y+";
                btnMRY.Text = "Y-";
                btnMRX.Text = "X-";
                btnARX.Text = "X+";
            }
            else
            {
                btnARZ.Text = "+";
                btnMRZ.Text = "-";
                btnARY.Text = "+";
                btnMRY.Text = "-";
                btnMRX.Text = "-";
                btnARX.Text = "+";
            }
        }

  



        //调试区
        private void axUnityWebPlayer1_OnExternalCall_1(object sender, AxUnityWebPlayerAXLib._DUnityWebPlayerAXEvents_OnExternalCallEvent e)
        {
            /*
            if (e.value.StartsWith("crash"))
            {
                crash = true;
              //  Debug.WriteLine("发生碰撞!");
                axWindowsMediaPlayer1.Ctlcontrols.play();
          
            }
            else if (e.value.StartsWith("NOcrash"))
            {
                // timer3.Enabled = false;
                crash = false;
                //Debug.WriteLine("解除碰撞!");
                axWindowsMediaPlayer1.Ctlcontrols.stop();
            }
            */
         
                Debug.WriteLine(e.value.ToString());
            




        }
        #region 肖志键自己写的代码




        #endregion

    }
}
